#!/usr/bin/env python
# -*- coding: utf-8 -*-

import multiprocessing
import rospy
import tf
import actionlib
import actionlib_msgs.msg
import sys
sys.path.insert(0, '/home/center/fdse_coffee/src/')
# rospy.loginfo(sys.path)
import roslibpy
import os
from rosplan_planning_system.ActionInterfacePy.RPActionInterface import RPActionInterface
from math import sin, cos
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
import actionlib
from actionlib_msgs.msg import *
from geometry_msgs.msg import Pose, Point, Quaternion
import time
sys.path.insert(0, '/home/center/fdse_coffee/src/jaka_pro/')
from jaka_pro.run_rosplan import Jarvis
import threading
import requests

# os.getcwd()
def is_make_coffee_success(): ...
def is_move_into_cabin_success(): ...
def is_place_cup_into_delivery_robot_success(): ...
def is_deliver_coffee_success(): ...
def is_go_chargeing_success(): ...

class RPMoveBasePy(RPActionInterface):
    def __init__(self):
        # call parent constructor
        RPActionInterface.__init__(self)

    def concreteCallback(self, msg):
        name = msg.name
        if(name == 'get_order'):
            '''
            @desc: get order from ui/pepper, upload to parameter server
            '''
            rospy.loginfo('Kcl: get order from ui/pepper, upload to parameter server')
            coffee_cate = 'USA'
            seat_num = 3
            ros_mini = roslibpy.Ros('192.168.64.29', 9090)
            ros_mini.run()
            ros_mini.set_param('/peanut/coffee_place', 'no')
            ros_mini.set_param('/peanut/ready', 'no')
            ros_mini.set_param('/order/ready', 'no')
            ros_mini.set_param('/order/coffee_cate', coffee_cate)
            ros_mini.set_param('/order/table_id', seat_num)
            ros_mini.set_param('/peanut/table_id', seat_num)

            return True

        elif(name == 'make_coffee'):
            '''
            @desc: arm grabs a cup, coffee machine starts to cook
            '''
            rospy.loginfo('KCL: make_coffee action start!')

            # action execution

            # robot_control = Jarvis(control_center_ip='192.168.1.133',
            #                 jaka_ip='192.168.1.144')
            # robot_control.make_coffee()
            # robot_control.pause_close()
            
            url = 'http://127.0.0.1:5001/get_cup/'
            requests.post(url)
            url = 'http://127.0.0.1:5001/make_coffee/'
            requests.post(url)

            # post-condition check
            is_make_coffee_success()

            return True
            
        elif(name == 'move_into_cabin'):
            '''
            @desc: peanut moves into cabin from anywhere, opens hatch
            '''

            # action execution

            url = 'http://127.0.0.1:5002/peanut_MoveIntoCabin/'
            requests.post(url)

            # post-condition check
            is_move_into_cabin_success()

            return True

        elif(name == 'place_cup_into_delivery_robot'):
            '''
            @desc: arm puts a cup to peanut 
            '''

            # action execution

            url = 'http://127.0.0.1:5002/peanut_WaitCup/'
            t1 = threading.Thread(target=requests.post,args=(url,))
            t2 = threading.Thread(target=self.place)
            #t1 = multiprocessing.Process(target=requests.post,args=(url,))
            #t2 = multiprocessing.Process(target=self.place)
            t1.start()
            t2.start()            
            t1.join()
            t2.join()

            # post-condition check
            is_place_cup_into_delivery_robot_success()

            return True
            

        elif(name == 'deliver_coffee'):
            '''
            @desc: peanut closes hatch, delivers to desk, opens hatch, closes hatch
            '''

            # action execution

            url = 'http://127.0.0.1:5002/peanut_DeliverCoffee/'
            requests.post(url)

            # post-condition check
            is_deliver_coffee_success()

            return True

        elif name== 'go_chargeing':

            # action execution

            url = 'http://127.0.0.1:5002/peanut_Charge/'
            requests.post(url)

            # post-condition check
            is_go_chargeing_success()

            return True

        else:
           return False


if __name__ == '__main__':
    rospy.init_node('rosplan_interface_movebase', anonymous=False)
    rpmb = RPMoveBasePy()
    rpmb.runActionInterface()

